#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from unitree_api.msg import Request
import json
import re  # 使用正则表达式处理标点

class VoiceMotionControl(Node):
    def __init__(self):
        super().__init__('voice_motion_control')
        
        # 运动控制参数
        self.declare_parameter("speed", 0.25)
        self.declare_parameter("angular", 0.7)
        self.speed = self.get_parameter("speed").value
        self.angular = self.get_parameter("angular").value
        
        # 语音命令到API的映射
        self.voice_commands = {
            "前进": ('move', (1, 0, 0)),
            "后退": ('move', (-1, 0, 0)),
            "左转": ('move', (0, 0, 1)),
            "右转": ('move', (0, 0, -1)),
            "站立": ('api', 1004),
            "坐下": ('api', 1009),
            "打招呼": ('api', 1016),
            "恢复站立": ('api', 1010),
            "卧倒": ('api', 1005)
        }
        
        # 发布器
        self.pub = self.create_publisher(Request, "/api/sport/request", 10)
        
        # 语音订阅
        self.voice_sub = self.create_subscription(
            String,
            '/voicewords',
            self.voice_callback,
            10)
        
        self.get_logger().info("语音运动控制节点已启动（仅语音控制模式）")

    def publish(self, api_id, x=0.0, y=0.0, z=0.0):
        """通用发布方法"""
        req = Request()
        req.header.identity.api_id = api_id
        req.parameter = json.dumps({"x": x, "y": y, "z": z})
        self.pub.publish(req)
        self.get_logger().info(f"已发送指令: API={api_id}, x={x}, y={y}, z={z}")

    def voice_callback(self, msg):
        """处理语音指令，移除所有标点符号（包括中文标点）并标准化"""
        # 原始指令
        raw_command = msg.data.strip()
        
        # 使用正则表达式移除所有标点符号（包括中文标点）
        # 匹配所有非中文字符和非英文字符的标点符号
        command = re.sub(r'[^\u4e00-\u9fa5a-zA-Z0-9]', '', raw_command)
        
        self.get_logger().info(f"收到语音指令: 原始='{raw_command}', 处理后='{command}'")
        
        if command in self.voice_commands:
            cmd_type, value = self.voice_commands[command]
            
            if cmd_type == 'move':
                # 运动控制指令
                x_bind, y_bind, z_bind = value
                self.publish(
                    1008,
                    x=x_bind * self.speed,
                    y=y_bind * self.speed,
                    z=z_bind * self.angular
                )
            elif cmd_type == 'api':
                # 直接API调用
                self.publish(value)
        else:
            self.get_logger().warn(f"未知指令: '{command}'")

def main(args=None):
    rclpy.init(args=args)
    node = VoiceMotionControl()
    
    try:
        # 启动ROS节点
        rclpy.spin(node)
    except KeyboardInterrupt:
        # 捕获Ctrl+C，让机器人卧倒
        node.publish(1005)
        node.get_logger().info("收到终止信号，机器人卧倒")
    except Exception as e:
        node.get_logger().error(f"节点错误: {str(e)}")
    finally:
        # 确保退出时机器人卧倒
        node.publish(1005)
        rclpy.shutdown()

if __name__ == '__main__':
    main()
    